//
// Created by 马皓然 on 2025/10/25.
//
#include "motor_manager.h"
#include <stdio.h> // 用于示例中的打印或错误提示

// 默认控制参数
#define DEFAULT_KP 100.0f
#define DEFAULT_KD 1.0f

/**
 * @brief  初始化电机实例结构体
 * @param  motor: 指向电机实例结构体的指针
 * @param  motor_id: 电机的 CAN ID (0x01, 0x02, ...)
 * @retval None
 */
void Motor_Manager_Init(Motor_Instance_t *motor, uint8_t motor_id) {
    motor->id = motor_id;
    motor->state = MOTOR_STATE_UNKNOWN;
    motor->target_pos = 0.0f;
    motor->target_vel = 0.0f;
    motor->kp = DEFAULT_KP;
    motor->kd = DEFAULT_KD;
}

/**
 * @brief  向电机发送使能运行命令 (CAN Mode 3)
 * @param  motor: 指向电机实例结构体的指针
 * @retval HAL_OK/HAL_ERROR
 */
uint8_t Motor_Manager_Enable(Motor_Instance_t *motor) {
    if (CyberGear_SendModeControl(&hcan1, motor->id, CYBERGEAR_MODE_ENABLE) == HAL_OK) {
        motor->state = MOTOR_STATE_ENABLED;
        // 成功发送，但电机实际进入运行状态需要时间
        return HAL_OK;
    }
    motor->state = MOTOR_STATE_FAULT;
    return HAL_ERROR;
}

/**
 * @brief  向电机发送停止运行命令 (CAN Mode 4)
 * @param  motor: 指向电机实例结构体的指针
 * @retval HAL_OK/HAL_ERROR
 */
uint8_t Motor_Manager_Stop(Motor_Instance_t *motor) {
    if (CyberGear_SendModeControl(&hcan1, motor->id, CYBERGEAR_MODE_STOP) == HAL_OK) {
        motor->state = MOTOR_STATE_STOPPED;
        return HAL_OK;
    }
    return HAL_ERROR;
}

/**
 * @brief  设置电机的运行模式 (写入参数 Index 0x7005)
 * @param  motor: 指向电机实例结构体的指针
 * @param  run_mode: 目标模式 (RUN_MODE_POSITION, RUN_MODE_SPEED, ...)
 * @retval HAL_OK/HAL_ERROR
 * @note   根据手册，切换模式前最好先发送停止运行命令。
 */
uint8_t Motor_Manager_Set_Mode(Motor_Instance_t *motor, uint8_t run_mode) {
    uint16_t index_run_mode = 0x7005;

    // 写入模式 (1字节数据)
    if (CyberGear_SendWriteParam(&hcan1, motor->id, index_run_mode, 1, &run_mode) == HAL_OK) {
        // 成功发送
        return HAL_OK;
    }
    return HAL_ERROR;
}

/**
 * @brief  周期性地发送运动控制指令 (CAN Mode 1)
 * @param  motor: 指向电机实例结构体的指针
 * @retval HAL_OK/HAL_ERROR
 * @note   此函数应在定时器中断或高频主循环中周期性调用。
 */
uint8_t Motor_Manager_Update_Control(Motor_Instance_t *motor) {
    if (motor->state != MOTOR_STATE_ENABLED && motor->state != MOTOR_STATE_RUNNING) {
        return HAL_ERROR; // 电机未处于可控制状态
    }

    CyberGear_MotionCmd_t cmd;

    // 1. 从电机实例中获取控制参数
    cmd.pos_ref = motor->target_pos;
    cmd.vel_ref = motor->target_vel;
    cmd.tor_ff = 0.0f; // 暂无力矩前馈
    cmd.kp = motor->kp;
    cmd.kd = motor->kd;

    // 2. 发送控制指令
    if (CyberGear_SendMotionCmd(&hcan1, motor->id, &cmd) == HAL_OK) {
        motor->state = MOTOR_STATE_RUNNING;
        return HAL_OK;
    }

    // 发送失败，保持当前状态，等待下一次尝试
    return HAL_ERROR;
}

